
using System;
using System.Collections;
using System.Collections.Generic;
using IrrlichtNETCP;
using IrrlichtNETCP.Extensions;
using Ode.NET;

namespace kinematix
{


	public enum JointType
	{
		Ball = 0,
		Hinge,
		Slider,
		Universal,
		Fixed
	}
	
	public enum JointParameter
	{
		BallAnchor = 0,
		SliderAxis
	}

	public class phBody
	{

		IntPtr body;
		d.Mass mass;
		IntPtr geom;
		
		IntPtr world;
		IntPtr space;
		
		IntPtr jointgroup;
		
		bool enabled;

		bool isTrimesh = false;
		
		int dis_steps_cur = 0;
		int dis_steps_given = 0;
		float dis_lvel = 0;
		float dis_avel = 0;
		
		int joint_count = 0;
		
		BodyType bodyType;

		IntPtr data; 		
		
		bool movable;
		
		Vector3D position;
		Vector3D euler;
		d.Matrix3 rotation;
		
		d.Matrix4 last_transform;
		float _mass;
		
		Dictionary<int, IntPtr> joints;

		public phBody(IntPtr _world, IntPtr _space, Vector3D pos, Vector3D size, BodyType type, bool mv, AnimatedMesh mesh)
		{
		
			world = _world;
			space = _space;
			
			bodyType = type;
			movable = mv;
			
			if (type == BodyType.TriangleMesh)
			{
				isTrimesh = true;
				last_transform = new d.Matrix4();
			}
			
			if (movable)
			{
			body = d.BodyCreate(world);
			mass = new d.Mass();		
			_mass = 1.0f;
			//d.BodySetPosition(body, pos.X, pos.Y, pos.Z);
			}
		
		
			switch (bodyType)
			{
				case BodyType.TriangleMesh:
					
					uint i, j;
					int ci, cif, cv;
					int indexcount = 0;
					int vertexcount = 0; 					
					Mesh omesh = mesh.GetMesh(0);
					MeshBuffer mb= omesh.GetMeshBuffer(0);
					
					vertexcount = mb.VertexCount;
					indexcount = mb.IndexCount;
					
					float[] vertices = new float[mb.VertexCount*3];
					cv = 0;
					for (i = 0; i<mb.VertexCount; i++)
					{
					vertices[cv] = mb.GetVertex(i).Position.X;
					vertices[cv+1] = mb.GetVertex(i).Position.Y;
					vertices[cv+2] = mb.GetVertex(i).Position.Z;
					cv+=3;
					//verts[i+3] = 0;								
				}
					
					int[] indices = new int[mb.IndexCount];
					for (i = 0; i<mb.IndexCount; i++)
					{
					indices[i] = (int)mb.Indices[i];
				}
					/*for (i = 0; i < omesh.MeshBufferCount; i++)
					{
					MeshBuffer mb = omesh.GetMeshBuffer((uint)i);
					indexcount += mb.IndexCount;
					vertexcount += mb.VertexCount;
				}
            // build structure for ode trimesh geom
					float[] vertices = new float[vertexcount*3];
					int[] indices = new int[indexcount];

            // fill trimesh geom
					ci = 0; // current index in the indices array
					cif = 0; // offset of the irrlicht-vertex-index in the vetices array
					cv = 0; // current index in the vertices array
					for (i = 0; i < omesh.MeshBufferCount; i++)
					{
					MeshBuffer mb = omesh.GetMeshBuffer((uint)i);

					// fill indices
					for (j = 0; j < mb.IndexCount; j++)
					{
						// scale the indices from multiple meshbuffers to single index array
						indices[ci] = cif + mb.GetIndex(j);
						ci++;
					}

                // update the offset for the next meshbuffer
					cif = cif + mb.VertexCount;

                // fill vertices
					if (mb.VertexType == VertexType.Standard)
					{
                    //Vertex3D* mb_vertices = (Vertex3D*)mb.Vertices;
						for (j = 0; j < mb.VertexCount; j++)
						{
							vertices[cv] = mb.GetVertex(j).Position.X;
							vertices[cv+1] = mb.GetVertex(j).Position.Y;
							vertices[cv+2] = mb.GetVertex(j).Position.Z;
							cv+=3;
						}
					}
					else if (mb.VertexType == VertexType.T2Coords)
					{
						for (j = 0; j < mb.VertexCount; j++)
						{
							vertices[cv] = mb.GetVertexT2(j).Position.X;
							vertices[cv+1] = mb.GetVertexT2(j).Position.Y;
							vertices[cv+2] = mb.GetVertexT2(j).Position.Z;
							cv+=3;
						}
					} 
				}*/
					
					//Console.WriteLine("From mono: "+mb.VertexCount);
					GC.KeepAlive(vertices);
					GC.KeepAlive(indices);
					GC.KeepAlive(data);							
					data = d.GeomTriMeshDataCreate();
						
					//d.GeomTriMeshDataBuildSimple(data, verts, mb.VertexCount, indices, mb.IndexCount);
					//Console.WriteLine(mb.VertexCount +" "+mb.IndexCount);
					d.GeomTriMeshDataBuildSingle(data, vertices, 3*sizeof(float), vertexcount, indices, indexcount, 3*sizeof(int));
	
//					d.GeomTriMeshDataBuildSimple(data, Vertices, 5, Indices, 12);  */
  
						
					geom = d.CreateTriMesh(space, data, null, null, null);
					
					d.GeomSetData(geom, data);
					
					if (movable)
					{
					//d.MassSetTrimesh(out mass, 1, geom);
					d.MassSetBox(out mass, _mass, size.X, size.Y, size.Z);
					d.BodySetMass(body, ref mass);							
				}
					
				break;			
					
				case BodyType.Box:
					if (movable)
					{
					d.MassSetBox(out mass, _mass, size.X, size.Y, size.Z);
					d.MassAdjust(ref mass, _mass);
				}
					
					//d.BodySetMass(body, ref mass);
					
					geom = d.CreateBox(space, size.X,
					                   size.Y,
					                   size.Z);
					
					break;
				case BodyType.Sphere:
					if (movable)
					{
					d.MassSetSphere(out mass, _mass, size.X);
					d.MassAdjust(ref mass, _mass);
					
					d.BodySetMass(body, ref mass);
				}
					geom = d.CreateSphere(space, size.X);
					break;
				case BodyType.Cylinder:
					if (movable)
					{
					d.MassSetCylinder(out mass, _mass, 1, size.X, size.Y);
					//d.MassAdjust(ref mass, 1);
					
					d.BodySetMass(body, ref mass);
				}
					geom = d.CreateCylinder(space, size.X, size.Y);
					
					break;
					
				case BodyType.Capsule:
					if (movable)
					{
					d.MassSetCapsule(out mass, _mass, 2, size.Y, size.Z);
					
					d.BodySetMass(body, ref mass);
				}	
					geom = d.CreateCapsule(space, size.X, size.Y);
					
					break;
					
					
			}
			if (movable)
			{
				d.GeomSetBody(geom, body);
				d.BodySetPosition(body, pos.X, pos.Y, pos.Z);				
			}
			
			d.GeomSetPosition(geom, pos.X, pos.Y, pos.Z);
			
			position = pos;
			rotation = new d.Matrix3();
			euler = new Vector3D();
			enabled = movable;
			
		}
		
		public void Destroy()
		{
			if (movable)
			{
				d.BodyDestroy(body);
			} 
			
			if (isTrimesh)
			{d.GeomTriMeshDataDestroy(data);}
			
			d.GeomDestroy(geom);
			
			if (jointgroup != IntPtr.Zero)
			{
				d.JointGroupDestroy(jointgroup);
			}
		}
		
		public void SetUserData(IntPtr data)
		{
			if (data != IntPtr.Zero)
				d.GeomSetData(geom, data);
		}
		
	
		/*public d.AABB BBox()
		{
				d.AABB bbox = new d.AABB();
				d.GeomGetAABB(body, out bbox);
				return bbox;
		}*/
		
		public void AddForce(Vector3D force)
		{
			if (movable&&enabled)
			{
				d.BodyEnable(body);
				d.BodyAddForce(body, force.X, force.Y, force.Z);
			}
		}
		
		
		public int AddJoint(phBody abody, JointType jtype)
		{
			if (jointgroup == IntPtr.Zero)
			{
				jointgroup = d.JointGroupCreate(10);
				joints = new Dictionary<int, IntPtr>();
			}
			
			IntPtr joint = IntPtr.Zero;
			
			IntPtr b2 = abody.Body != IntPtr.Zero? abody.Body: IntPtr.Zero;
			
			d.Vector3 pos1 = d.BodyGetPosition(body);
			d.Vector3 pos2;
			
			if (abody.Movable)
			{
				pos2 = d.BodyGetPosition(b2);
			} else { pos2 = d.GeomGetPosition(abody.Geom);}
			
			
			switch (jtype) {
				
				case JointType.Fixed:
					
					joint = d.JointCreateFixed(world, IntPtr.Zero);
					if (movable)
					{
					d.JointAttach(joint, body, b2);
				} else {
					d.JointAttach(joint, body, IntPtr.Zero);
				}
					d.JointSetFixed(joint);			
					break;
				case JointType.Ball:
					
					joint = d.JointCreateBall(world, IntPtr.Zero);
					if (movable)
					{
					d.JointAttach(joint, body, b2);
				} else {
					d.JointAttach(joint, body, IntPtr.Zero);
				}
					break;
				case JointType.Slider:
					joint = d.JointCreateSlider(world, IntPtr.Zero);
					if (movable)
					{
					d.JointAttach(joint, body, b2);
				} else {
					d.JointAttach(joint, body, IntPtr.Zero);
				}				
					break;
			}
			
			Console.WriteLine((pos2.X - pos1.X)/2 +" "+ (pos2.Y - pos1.Y)/2 +" "+
			                  (pos2.Z - pos1.Z)/2);
			
			//d.JointSetBallAnchor(joint, (pos2.X - pos1.X)/2, (pos2.Y - pos1.Y)/2,
			//(pos2.Z - pos1.Z)/2);
			joint_count++;
			
			joints.Add(joint_count, joint);
			return joint_count;
			
		}
		
		
		public void JointSetParameter(JointParameter param, int joint, Vector3D values)
		{
			IntPtr j;
			switch (param)
			{
				case JointParameter.BallAnchor:
					if (joints.TryGetValue(joint, out j))
					{
					d.JointSetBallAnchor(j, values.X, values.Y, values.Z);
				}
					break;
				case JointParameter.SliderAxis:
					if (joints.TryGetValue(joint, out j)){
					d.JointSetSliderAxis(j, values.X, values.Y, values.Z);
				}					
					break;
			}
		}
		
		public IntPtr Body
		{
			get
			{
				if (movable)
				{
					return body;
				} else {
					return IntPtr.Zero;
				}
			}
		}
		
		public float Mass
		{
			get
			{
				return _mass;
			}
			set
			{
				if (movable)
				{
					d.MassAdjust(ref mass, value);
				}
			}
		}
		
		public bool Movable
		{
			get
			{
				return movable;
			}
		}
		
		public bool Enabled
		{
			get
			{
				if (movable){ 
					return enabled;} else { return false; };
			}
			set
			{
				if (movable)
				{
					enabled = value;
					if (enabled)
					{
						d.Vector3 pos = d.GeomGetPosition(geom);
						d.Matrix3 rot = d.GeomGetRotation(geom);
						d.BodySetPosition(body,pos.X,pos.Y,pos.Z);
						d.BodySetRotation(body, ref rot);
						d.GeomSetBody(geom, body);
					} else {
						d.GeomSetBody(geom, IntPtr.Zero);						
					}
				}
			}
		}
		
		public void Update()
		{
			if (movable)
			{
				DisableCheck();
			}
			
			
			d.Vector3 apos = d.GeomGetPosition(geom);
			position.X = apos.X;
			position.Y = apos.Y;
			position.Z = apos.Z;	
			
			if (movable)
			{
			d.Quaternion q = d.BodyGetQuaternion(body);
			euler = kHelp.Quat2Rot(q);
			}
			
	
		}
		
		public void PostUpdate()
		{
			if (bodyType == BodyType.TriangleMesh)
			{
				if (movable)
				{
					d.Vector3 apos = d.GeomGetPosition(geom);					
					if (bodyType == BodyType.TriangleMesh)
					{
						d.Matrix3 mat = d.BodyGetRotation(body);
						last_transform.M00 = mat.M00; last_transform.M01 = mat.M10; last_transform.M20 = mat.M02; last_transform.M03 = 0;
						last_transform.M10 = mat.M01; last_transform.M11 = mat.M11; last_transform.M21 = mat.M12; last_transform.M13 = 0;
						last_transform.M20 = mat.M02; last_transform.M21 = mat.M12; last_transform.M22 = mat.M22; last_transform.M23 = 0;
						last_transform.M30 = apos.X; last_transform.M31 = apos.Y;   last_transform.M32 = apos.Z; last_transform.M33 = 1;
					}
				}				
				d.GeomTriMeshSetLastTransform(geom, ref last_transform); 
			}
		}
		
		public Vector3D Position
		{
			get{
				return position;
			}
			set{
				position = value;
				if (movable&&enabled)
				{
					d.BodySetPosition(body, position.X,
					                  position.Y,
					                  position.Z);
				} else {
					d.GeomSetPosition(geom, position.X,
					                  position.Y,
					                  position.Z);
				}
			}
		}
		
		public Vector3D Rotation
		{
			get{
				return euler;
			}
			set{
				euler = value;
				Matrix4 mat = new Matrix4();
				mat.RotationDegrees = euler;
				rotation.M00 = mat.GetM(0,0); rotation.M01 = mat.GetM(1,0); rotation.M02 = mat.GetM(2,0);
				rotation.M10 = mat.GetM(0,1); rotation.M11 = mat.GetM(1,1); rotation.M12 = mat.GetM(2,1);						rotation.M20 = mat.GetM(0,2); rotation.M21 = mat.GetM(1,2); rotation.M22 = mat.GetM(2,2);
/*					Quaternion q = new Quaternion();
					q.Set(value.X, value.Y, value.Z);
					d.Quaternion _q = new d.Quaternion();
					_q.X = q.X;
					_q.Y = q.Y;
					_q.Z = q.Z;
					_q.W = q.W;				*/
				if (movable)
				{
					d.BodySetRotation(body, ref rotation);
				} else {
					d.GeomSetRotation(geom, ref rotation);
				}
				
			}
		}
		
		public float Velocity
		{
			get{
				if (movable&&enabled)
				{
					d.Vector3 v = d.BodyGetLinearVel(body);
					return (float)Math.Sqrt(v.X*v.X+v.Y*v.Y+v.Z*v.Z);
				}
				return 0;
			}
		}
		
		public void DisableCheck()
		{
			if (!kHelp.CheckForDisabling (body, dis_lvel, dis_avel))
				dis_steps_cur = 0;
			if (dis_steps_cur < dis_steps_given) {
				if (!kHelp.CheckForDisabling (body, dis_lvel, dis_avel)) {
					dis_steps_cur = 0;
					return;
				}
				dis_steps_cur++;
				if (dis_steps_cur >= dis_steps_given)
					d.BodyDisable(body);
			}
			if (bodyType == BodyType.TriangleMesh)
			{
				//d.GeomTriMeshClearTCCache(geom);
				d.Vector3 Pos = d.GeomGetPosition(geom);
				d.Matrix3 Rot = d.GeomGetRotation(geom);			
				d.GeomTriMeshSetLastTransform(geom, ref last_transform);
			}
		}
		
		public void SetDisableParams (float lvel, float avel, int steps)
		{
			dis_steps_given = steps;
			dis_lvel = lvel;
			dis_avel = avel;
		}
		
		
		public IntPtr Geom
		{
			get
			{
				return geom;
			}
		}
		
		
	}
}
